/***********************************************************************************
 * 文件名： pid_static.c
 * 作者： 	刘言
 * 版本： 	2
 * 说明：
 * 		PID算法计算器，静态时现（单例模式），一个项目中需要用到多组PID参数请使用面向对
 * 	象设计的pid.c。
 * 		PID 计算方法： 控制（输出）值的变化量 = 
 * 				  比例系数 * 偏差值 
 * 				+ 积分系数 * 偏差累加值 
 * 				+ 微分系数 * (偏差值 - 上一个偏差值)
 * 		
 * 修改记录：
 * 	20201106: 初版
 *  20220622: 预处理计算确定最合适的变量类型。
***********************************************************************************/
#include "pid_static.h"
#include "polsys.h"

// 确定计算时中间临时变量的类型
#ifdef _PID_USE_FLOAT
	typedef float ctrl_value_pd_t;
	typedef float ctrl_value_i_t;
#else
	#define PID_CTRL_PD_MAX	(_PID_KP * _PID_INPUT_MAX)
	#define PID_CTRL_I_MAX	(_PID_KI * _PID_INTE_MAX)

	#if (PID_CTRL_PD_MAX <= 0X7F)
		typedef s8 ctrl_value_pd_t;
	#elif (PID_CTRL_PD_MAX <= 0X7FFF)
		typedef s16 ctrl_value_pd_t;
	#elif (PID_CTRL_PD_MAX <= 0X7FFFFFFF)
		typedef s32 ctrl_value_pd_t;
	#else
		#error "参数设置的不合理导致中间临时变量过大。"
	#endif

	#if (PID_CTRL_I_MAX <= 0X7F)
		typedef s8 ctrl_value_i_t;
	#elif (PID_CTRL_I_MAX <= 0X7FFF)
		typedef s16 ctrl_value_i_t;
	#elif (PID_CTRL_I_MAX <= 0X7FFFFFFF)
		typedef s32 ctrl_value_i_t
	#else
		#error "参数设置的不合理导致中间临时变量过大。"
	#endif
#endif


static pdata input_value_t mLastDeviationValue;		// 上一个偏差值
static pdata inte_value_t mIntegralValue;			// 偏差累加值（积分值）

#ifdef _PID_USE_DYNAMIC_INTEGRAL	// 启用动态积分
static bit mArrive;		// 已达到标志
#endif
void PID_Init()
{
	mLastDeviationValue = 0;
	mIntegralValue = 0;
#ifdef _PID_USE_DYNAMIC_INTEGRAL	// 启用动态积分
	mArrive = false;
#endif
}


ctrl_value_t PID_GetChange(input_value_t target, input_value_t real)
{
	ctrl_value_t change;			// 控制量变化值
	input_value_t deviation_value;	// 偏差值
#ifdef _PID_USE_DYNAMIC_INTEGRAL	// 启用动态积分
	u8 IntegralRatio;		// 积分起作用的比例值
	
	deviation_value = target - real;	//偏差值，有符号
	#ifdef _PID_NO_INTEGRAL_IN_START		//启动时关闭积分作用
	if(mArrive == false)	//本次启动没有达到过目标值
	{
		IntegralRatio=0;		//关闭积分作用
		if(deviation_value<0)mArrive=true;	//标记达到目标值了
	}
	else
	#endif
	{
		// u16 temp;
		// temp = GetAbsS16(deviation_value);	//偏差绝对值
		// if(temp>_PID_NO_INTEGRAL_VALUE)		//偏差过大关闭积分作用
		// {
		// 	IntegralRatio=0;
		// }
		// else if(temp<_PID_FULL_INTEGRAL_VALUE)//偏差很小，正常积分
		// {
			IntegralRatio=1;
			mIntegralValue += deviation_value;
		// }
		// else		//偏差偏大，积分减弱
		// {
		// 	IntegralRatio=(_PID_NO_INTEGRAL_VALUE-temp)*_PID_SCALE/(_PID_NO_INTEGRAL_VALUE-_PID_FULL_INTEGRAL_VALUE);
		// 	mIntegralValue += deviation_value;					//累加偏差值，有符号
		// }
	}
#else 	// 关闭动态积分，始终完全积分
	deviation_value = target - real;	// 偏差值，有符号
	mIntegralValue += deviation_value;	// 累加偏差值，有符号
#endif
	// 限制积分饱和
	if(mIntegralValue > _PID_MAX_INTE_VALUE) mIntegralValue = _PID_MAX_INTE_VALUE;
	if(mIntegralValue < _PID_MIN_INTE_VALUE) mIntegralValue = _PID_MIN_INTE_VALUE;
	// 计算控制值
#ifdef _PID_USE_DYNAMIC_INTEGRAL
	#ifdef _PID_USE_FLOAT
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue * IntegralRatio
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
				);
	#else
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
		#if (_PID_KI != 0)
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue * IntegralRatio
		#endif
		#if (_PID_KD != 0)
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
		#endif
				) / _PID_SCALE;
	#endif
#else
	#ifdef _PID_USE_FLOAT
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
				);
	#else
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
		#if (_PID_KI != 0)
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue
		#endif
		#if (_PID_KD != 0)
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
		#endif
				) / _PID_SCALE;
	#endif
#endif
	//保存本次差值
	mLastDeviationValue = deviation_value;	

	return change;
}



